;void ondas_c (
;	unsigned char *src, RDI
;	unsigned char *dst, RSI
;	int m, EDX
;	int n, ECX
;	int row_size, R8d
;	int x0, R9d
;	int y0 pila (pop a R10d)
;) {

global ondas_asm

section .data

mascaraUno: DD 12.0, 13.0, 14.0, 15.0
mascaraDos: DD 8.0, 9.0, 10.0, 11.0
mascaraTres: DD 4.0, 5.0, 6.0, 7.0
mascaraCuatro: DD 0.0, 1.0, 2.0, 3.0

PI: DD 3.1415, 3.1415, 3.1415, 3.1415
RADIUS: DD 35.0, 35.0, 35.0, 35.0
WAVELENGTH: DD 64.0, 64.0, 64.0, 64.0
TRAINWIDTH: DD 3.4, 3.4, 3.4, 3.4

UNO: DD 1.0, 1.0, 1.0, 1.0
DOS: DD 2.0, 2.0, 2.0, 2.0
F_6: DD 6.0, 6.0, 6.0, 6.0
F_120: DD 120.0, 120.0, 120.0, 120.0
F_5040: DD 5040.0, 5040.0, 5040.0, 5040.0
F_64: DD 64.0, 64.0, 64.0, 64.0

I_255: DD 255, 255, 255, 255

section .text

ondas_asm:

	push RBP		; impar |
	mov RBP, RSP		; armo el stackframe
	push R15		; par | R15 va a ser el contador de filas
	push R14		; impar |R14 va a ser el contador de columnas
	push R10		; par |en R10d voy a tener y0
	sub RSP, 8		; alineo la pila
	xor R15, R15		; R15 = 0
	xor R14, R14		; R14 = 0
	xor R10, R10		; R10 = 0
	mov R10d, [RBP+16]	; R10d = y0
	mov EAX, R8d		; EAX = row_size
	xor R8, R8		; limpio R8
	mov R8d, EAX		; vuelvo a poner en R8 el row_size

	; SETEO POR UNICA VEZ XMM15 = (float)|x0|x0|x0|x0|

		pxor XMM15, XMM15
		movd XMM15, R10d
		cvtdq2ps XMM15, XMM15	; XMM15 = (float)x0
		pxor XMM13, XMM13
		movdqu XMM13, XMM15
		pslldq XMM15, 4
		addps XMM15, XMM13
		pslldq XMM15, 4
		addps XMM15, XMM13
		pslldq XMM15, 4
		addps XMM15, XMM13	; XMM15 = (float)|x0|x0|x0|x0|

	; SETEO POR UNICA VEZ XMM14 = (float)|y0|y0|y0|y0|

		pxor XMM14, XMM14
		movd XMM14, R9d
		cvtdq2ps XMM14, XMM14	; XMM14 = (float)y0
		pxor XMM13, XMM13
		movdqu XMM13, XMM14
		pslldq XMM14, 4
		addps XMM14, XMM13
		pslldq XMM14, 4
		addps XMM14, XMM13
		pslldq XMM14, 4
		addps XMM14, XMM13	; XMM14 = (float)|y0|y0|y0|y0|

	; ---------------- ARRANCA EL PROCESAMIENTO DE LA MATRIZ ----------------------------;


.ciclo_filas:
	cmp R15d, EDX			; me fijo si recorri todas las filas
	je .fin_ciclo_filas		; si no las recorri sigo ciclando


	pxor XMM4, XMM4											
	pxor XMM12, XMM12										
	movd XMM4, R15d											
	paddd XMM12, XMM4										
	pslldq XMM12, 4											
	paddd XMM12, XMM4										
	pslldq XMM12, 4											
	paddd XMM12, XMM4										
	pslldq XMM12, 4											
	paddd XMM12, XMM4										
	pslldq XMM12, 4											
	paddd XMM12, XMM4	; XMM12 = |filaActual|filaActual|filaActual|filaActual|	
	cvtdq2ps XMM12, XMM12	; XMM12 = (float)|filaActual|filaActual|filaActual|filaActual|

	subps XMM12, XMM14	; XMM12 = |filaActual|filaActual|filaActual|filaActual| - |y0|y0|y0|y0|
	mulps XMM12, XMM12	; XMM12 = |((filaActual)-y0)^2|((filaActual)-y0)^2|((filaActual)-y0)^2|((filaActual)-y0)^2|

	.ciclo_columnas:
		
		add R14d, 16		; al sumar 16 me fijo si voy a poder levantar una tira entera
		cmp R14d, ECX		; me fijo si recorri todas las columnas de esta fila
		jg .fin_ciclo_columnas 	; si no las recorri sigo ciclando
		sub R14d, 16		; acomodo R14 a su valor orginal
		
		movdqu XMM0, [RDI+R14]	; levanto 16 bytes, pixeles tambien en este caso

		;----------------- PROCESAMIENTO -----------------;
		; valores a tener en cuenta:
		; XMM15 = (float)|x0|x0|x0|x0|
		; XMM14 = (float)|y0|y0|y0|y0|
		; XMM13 = (float)|colActual|colActual|colActual|colActual|
		; XMM12 = |((filaActual)-y0)^2|((filaActual)-y0)^2|
		; XMM0 = 16 bytes levantados
		; XMM1 = 16 bytes a setear, se va armando durante el procesamiento
		; XMM4 auxiliar para operaciones
		
		pxor XMM13, XMM13
		pxor XMM4, XMM4
		movd XMM4, R14d
		paddd XMM13, XMM4
		pslldq XMM13, 4
		paddd XMM13, XMM4
		pslldq XMM13, 4
		paddd XMM13, XMM4
		pslldq XMM13, 4
		paddd XMM13, XMM4
		pslldq XMM13, 4	
		paddd XMM13, XMM4	; XMM13 = |colActual|colActual|colActual|colActual|
		cvtdq2ps XMM13, XMM13	; XMM13 = (float)|colActual|colActual|colActual|colActual|

		pxor XMM1, XMM1		; limpio la salida

		; ######################################################################################## ;
		; PROCESO COLUMNAS |ACTUAL + 12|ACTUAL + 13|ACTUAL + 14|ACTUAL + 15|

		movups XMM2, XMM13	; XMM2 = (float)|colActual|colActual|colActual|colActual|
	
		movups XMM4, [mascaraUno]
		addps XMM2, XMM4	; XMM2 = (float)|colActual_0|colActual_1|colActual_2|colActual_3|

		call profundidad
		
		movups XMM4, [F_64]
		mulps XMM2, XMM4	; XMM2 = |prof_0 * 64|prof_1 * 64|prof_2 * 64|prof_3 * 64|
		
		pxor XMM4, XMM4
		movdqu XMM10, XMM0
		punpckhbw XMM10, XMM4
		punpckhwd XMM10, XMM4
		cvtdq2ps XMM10, XMM10
		addps XMM2, XMM10	; XMM2 = |new_pixel_0|new_pixel_1|new_pixel_2|new_pixel_3|
		cvtps2dq XMM2, XMM2	; XMM2 = (int)|new_pixel_0|new_pixel_1|new_pixel_2|new_pixel_3|

		pxor XMM4, XMM4
		pcmpgtw XMM4, XMM2
		pandn XMM4, XMM2
		movdqu XMM2, XMM4	; XMM2 = new_pixel <   0.0 ?   0.0 : new_pixel;

		movdqu XMM4, [I_255]
		pcmpgtw XMM4, XMM2
		pand XMM2, XMM4
		movdqu XMM3, [I_255]
		pandn XMM4, XMM3
		paddd XMM2, XMM4	; XMM2 = new_pixel > 255.0 ? 255.0 : new_pixel;

		pxor XMM4, XMM4
		packssdw XMM2, XMM4
		packuswb XMM2, XMM4

		paddb XMM1, XMM2
		pslldq XMM1, 4

		; FIN PROCESO COLUMNAS |ACTUAL + 12|ACTUAL + 13|ACTUAL + 14|ACTUAL + 15|
		; ######################################################################################## ;


		; ######################################################################################## ;
		; PROCESO COLUMNAS |ACTUAL + 8|ACTUAL + 9|ACTUAL + 10|ACTUAL + 11|

		movups XMM2, XMM13	; XMM2 = (float)|colActual|colActual|colActual|colActual|
	
		movups XMM4, [mascaraDos]
		addps XMM2, XMM4	; XMM2 = (float)|colActual_0|colActual_1|colActual_2|colActual_3|

		call profundidad
		
		movups XMM4, [F_64]
		mulps XMM2, XMM4	; XMM2 = |prof_0 * 64|prof_1 * 64|prof_2 * 64|prof_3 * 64|
		
		pxor XMM4, XMM4
		movdqu XMM10, XMM0
		punpckhbw XMM10, XMM4
		punpcklwd XMM10, XMM4
		cvtdq2ps XMM10, XMM10
		addps XMM2, XMM10	; XMM2 = |new_pixel_0|new_pixel_1|new_pixel_2|new_pixel_3|
		cvtps2dq XMM2, XMM2	; XMM2 = (int)|new_pixel_0|new_pixel_1|new_pixel_2|new_pixel_3|

		pxor XMM4, XMM4
		pcmpgtw XMM4, XMM2
		pandn XMM4, XMM2
		movdqu XMM2, XMM4	; XMM2 = new_pixel <   0.0 ?   0.0 : new_pixel;

		movdqu XMM4, [I_255]
		pcmpgtw XMM4, XMM2
		pand XMM2, XMM4
		movdqu XMM3, [I_255]
		pandn XMM4, XMM3
		paddd XMM2, XMM4	; XMM2 = new_pixel > 255.0 ? 255.0 : new_pixel;

		pxor XMM4, XMM4
		packssdw XMM2, XMM4
		packuswb XMM2, XMM4

		paddb XMM1, XMM2
		pslldq XMM1, 4

		; FIN PROCESO COLUMNAS |ACTUAL + 8|ACTUAL + 9|ACTUAL + 10|ACTUAL + 11|
		; ######################################################################################## ;

		; ######################################################################################## ;
		; PROCESO COLUMNAS |ACTUAL + 4|ACTUAL + 5|ACTUAL + 6|ACTUAL + 7|

		movups XMM2, XMM13	; XMM2 = (float)|colActual|colActual|colActual|colActual|
	
		movups XMM4, [mascaraTres]
		addps XMM2, XMM4	; XMM2 = (float)|colActual_0|colActual_1|colActual_2|colActual_3|

		call profundidad
		
		movups XMM4, [F_64]
		mulps XMM2, XMM4	; XMM2 = |prof_0 * 64|prof_1 * 64|prof_2 * 64|prof_3 * 64|
		
		pxor XMM4, XMM4
		movdqu XMM10, XMM0
		punpcklbw XMM10, XMM4
		punpckhwd XMM10, XMM4
		cvtdq2ps XMM10, XMM10
		addps XMM2, XMM10	; XMM2 = |new_pixel_0|new_pixel_1|new_pixel_2|new_pixel_3|
		cvtps2dq XMM2, XMM2	; XMM2 = (int)|new_pixel_0|new_pixel_1|new_pixel_2|new_pixel_3|

		pxor XMM4, XMM4
		pcmpgtw XMM4, XMM2
		pandn XMM4, XMM2
		movdqu XMM2, XMM4	; XMM2 = new_pixel <   0.0 ?   0.0 : new_pixel;

		movdqu XMM4, [I_255]
		pcmpgtw XMM4, XMM2
		pand XMM2, XMM4
		movdqu XMM3, [I_255]
		pandn XMM4, XMM3
		paddd XMM2, XMM4	; XMM2 = new_pixel > 255.0 ? 255.0 : new_pixel;

		pxor XMM4, XMM4
		packssdw XMM2, XMM4
		packuswb XMM2, XMM4

		paddb XMM1, XMM2
		pslldq XMM1, 4

		; FIN PROCESO COLUMNAS |ACTUAL + 4|ACTUAL + 5|ACTUAL + 6|ACTUAL + 7|
		; ######################################################################################## ;


		; ######################################################################################## ;
		; PROCESO COLUMNAS |ACTUAL + 0|ACTUAL + 1|ACTUAL + 2|ACTUAL + 3|

		movups XMM2, XMM13	; XMM2 = (float)|colActual|colActual|colActual|colActual|
	
		movups XMM4, [mascaraCuatro]
		addps XMM2, XMM4	; XMM2 = (float)|colActual_0|colActual_1|colActual_2|colActual_3|

		call profundidad
		
		movups XMM4, [F_64]
		mulps XMM2, XMM4	; XMM2 = |prof_0 * 64|prof_1 * 64|prof_2 * 64|prof_3 * 64|
		
		pxor XMM4, XMM4
		movdqu XMM10, XMM0
		punpcklbw XMM10, XMM4
		punpcklwd XMM10, XMM4
		cvtdq2ps XMM10, XMM10
		addps XMM2, XMM10	; XMM2 = |new_pixel_0|new_pixel_1|new_pixel_2|new_pixel_3|
		cvtps2dq XMM2, XMM2	; XMM2 = (int)|new_pixel_0|new_pixel_1|new_pixel_2|new_pixel_3|

		pxor XMM4, XMM4
		pcmpgtw XMM4, XMM2
		pandn XMM4, XMM2
		movdqu XMM2, XMM4	; XMM2 = new_pixel <   0.0 ?   0.0 : new_pixel;

		movdqu XMM4, [I_255]
		pcmpgtw XMM4, XMM2
		pand XMM2, XMM4
		movdqu XMM3, [I_255]
		pandn XMM4, XMM3
		paddd XMM2, XMM4	; XMM2 = new_pixel > 255.0 ? 255.0 : new_pixel;

		pxor XMM4, XMM4
		packssdw XMM2, XMM4
		packuswb XMM2, XMM4

		paddb XMM1, XMM2


		; FIN PROCESO COLUMNAS |ACTUAL + 0|ACTUAL + 1|ACTUAL + 2|ACTUAL + 3|
		; ######################################################################################## ;



		;---------------- ! PROCESAMIENTO ----------------;
		
		movdqu [RSI+R14], XMM1	; seteo 16 pixeles
	
		add R14d, 16		; incremento en 16 el contador de columnas
		jmp .ciclo_columnas

	.fin_ciclo_columnas:

	sub R14d, 16			; acomodo R14 a su valor original
	cmp R14d, ECX			; me fijo si procese todas las columnas
	je .multiplo_16			; o las columnas eran multiplos de 16 o ya pase por aca y acomode los indices
	mov R14d, ECX			; seteo el contador de columnas en cantidad de columnas de la foto
	sub R14d, 16			; acomodo el contador para el ultimo procesamiento de la fila
	jmp .ciclo_columnas		; proceso la ultima levantada

.multiplo_16:
	add RDI, R8			; src = src + row size, paso de fila
	add RSI, R8			; dst = dst + row size, paso de fila
	xor R14, R14			; reseteo el contador de columnas
	inc R15d			; incremento el contador de filas en 1
	jmp .ciclo_filas
		
.fin_ciclo_filas:

.fin:
	add RSP, 8			; alineo la pila
	pop R10
	pop R14
	pop R15
	pop RBP
	ret



profundidad:

	subps XMM2, XMM15	; XMM2 = |x_0|x_1|x_2|x_3| - |x0|x0|x0|x0|
	mulps XMM2, XMM2	; XMM2 = |((colActual_0)-x0)^2|((colActual_1)-x0)^2|
				;	 |((colActual_2)-x0)^2|((colActual_3)-x0)^2|

	addps XMM2, XMM12	; XMM2 = |((colActual_0)-x0)^2 + y0|((colActual_1)-x0)^2 + y0|
				;	 |((colActual_2)-x0)^2 + y0|((colActual_3)-x0)^2 + y0|

	sqrtps XMM2, XMM2	; XMM2 = |sqrt(XMM2)|sqrt(XMM2)|sqrt(XMM3)|sqrt(XMM3)|
				; XMM2 = |dxy_0|dxy_1|dxy_2|dxy_3|

	movups XMM4, [RADIUS] 
	subps XMM2, XMM4 	; XMM2 = |dxy_0 - RADIUS|dxy_1 - RADIUS|dxy_2 - RADIUS|dxy_3 - RADIUS|
				; XMM2 = | r_0 | r_1 | r_2 | r_3 |


	movups XMM4, [WAVELENGTH]
	divps XMM2, XMM4	; XMM2 = |r_0/WAVELENGTH|r_1/WAVELENGTH|r_2/WAVELENGTH|r_3/WAVELENGTH|
				; XMM2 = | r_0 | r_1 | r_2 | r_3 |
	movups XMM8, XMM2	; XMM8 = | r_0 | r_1 | r_2 | r_3 |

	roundps XMM4, XMM2, 1	; XMM4 = floor(XMM2)
	subps XMM2, XMM4	; XMM2 = XMM2 - floor(XMM2)
				; XMM2 = | k_0 | k_1 | k_2 | k_3 |

	movups XMM4, [TRAINWIDTH]
	divps XMM8, XMM4	; XMM8 = | r_0 / TRAINWIDTH| r_1 / TRAINWIDTH | r_2 / TRAINWIDTH | r_3 / TRAINWIDTH |
	mulps XMM8, XMM8	; XMM8 = | (r_0 / TRAINWIDTH)^2 | (r_1 / TRAINWIDTH)^2 | (r_2 / TRAINWIDTH)^2 | (r_3 / TRAINWIDTH)^2 |

	movups XMM4, [UNO]
	addps XMM8, XMM4	; XMM8 = | (r_0 / TRAINWIDTH)^2 + 1 | (r_1 / TRAINWIDTH)^2 + 1| 
				;	 | (r_2 / TRAINWIDTH)^2 + 1 | (r_3 / TRAINWIDTH)^2 + 1|

	divps XMM4, XMM8	
	movups XMM8, XMM4	; XMM8 = (1.0)/XMM2
				; XMM8 = | a_0 | a_1 | a_2 | a_3 |


	movups XMM4, [DOS]
	mulps XMM2, XMM4	; XMM2 = | r_0 * 2 | r_1 * 2 | r_2 * 2 | r_3 * 2 |
	movups XMM4, [PI]
	mulps XMM2, XMM4	; XMM2 = | r_0 * 2 * PI | r_1 * 2 * PI | r_2 * 2 * PI | r_3 * 2 * PI |
	subps XMM2, XMM4	; XMM2 = | (r_0 * 2 * PI)-PI | (r_1 * 2 * PI)-PI | (r_2 * 2 * PI)-PI | (r_3 * 2 * PI)-PI |
				; XMM2 = | t_0 | t_1 | t_2 | t_3 |


	;--------- SIN TAYLOR ---------;
	; XMM2 = |x|x|x|x|
	; XMM5 va a ser |x_3|x_3|x_3|x_3|
	; XMM6 va a ser |x_5|x_5|x_5|x_5|
	; XMM7 va a ser |x_7|x_7|x_7|x_7|

	movups XMM5, XMM2
	mulps XMM5, XMM2
	mulps XMM5, XMM2	; XMM5 = |x_3|x_3|x_3|x_3| -> x*x*x
	movups XMM6, XMM5
	movups XMM4, [F_6]
	divps XMM5, XMM4	; XMM5 = |x_3/6|x_3/6|x_3/6|x_3/6|

	mulps XMM6, XMM2
	mulps XMM6, XMM2	; XMM6 = |x_5|x_5|x_5|x_5| -> x*x*x*x*x
	movups XMM7, XMM6
	movups XMM4, [F_120]
	divps XMM6, XMM4	; XMM6 = |x_5/120|x_5/120|x_5/120|x_5/120|

	mulps XMM7, XMM2
	mulps XMM7, XMM2	; XMM7 = |x_7|x_7|x_7|x_7| -> x*x*x*x*x*x*x
	movups XMM4, [F_5040]
	divps XMM7, XMM4	; XMM7 = |x_7/5040|x_7/5040|x_7/5040|x_7/5040|

	subps XMM2, XMM5	; XMM2 = x - (x_3/6) 4 veces	
	addps XMM2, XMM6	; XMM2 = x - (x_3/6) + (x_5/120) 4 veces
	subps XMM2, XMM7	; XMM2 = x - (x_3/6) + (x_5/120) - (x_7/5040) 4 veces
				; XMM2 = |s_taylor_0|s_taylor_1|s_taylor_2|s_taylor_3|

	;-------- ! SIN TAYLOR --------;

	mulps XMM2, XMM8	; XMM2 = |s_taylor_0 * a_0|s_taylor_1 * a_1|s_taylor_2 * a_2|s_taylor_3 * a_3|

	ret
